package gsw.tool.arithmetic.astar;

import java.util.ArrayList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Stack;

import gsw.tool.common.Tool;
import gsw.tool.common.ToolShow;


/**
 * A-Star（A*)算法是一种静态路网中求解最短路最有效的方法。它使用估价函数表示走下一个点时的路程，然后从能走的点中选取路程最小的那个点开始走，直到走到终点。估价函数公式表示为：f(n)=g(n)+h(n)，其中f(n)是节点n从初始点到目标点的估价函数，g(n)是在状态空间中从初始节点到n节点的实际代价，h(n)是从n到目标节点最佳路径的估计代价。
 * A-Star算法分为简化版和完整版。
 * A-Star算法的应用：游戏找路。
 */
public class AStar {
    // 表示终点
    private static int PATH = 2;
    // 障碍物
    private static int BAR = 3;
    // 横向纵向移动一步的距离
    private final static int DIRECT_VALUE = 10;
    // 斜向移动一步的距离
    private final static int OBLIQUE_VALUE = 14;

    /**
     * 测试A*算法
     */
    public static void test() {
        Node start = new Node(1, 1);
        Node end = new Node(6, 2);
        //模拟地图
        int[][] arr = new int[8][18];
        arr[0] = new int[]{3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
        arr[1] = new int[]{3, 0, 0, 0, 0, 0, 0, 0, 3, 3, 0, 0, 0, 3, 3, 0, 3, 3};
        arr[2] = new int[]{3, 3, 3, 0, 3, 3, 3, 0, 3, 3, 0, 3, 0, 0, 0, 0, 3, 3};
        arr[3] = new int[]{3, 3, 3, 0, 3, 3, 3, 0, 0, 0, 0, 3, 0, 3, 3, 3, 3, 3};
        arr[4] = new int[]{3, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 0, 0, 0, 0, 3, 3, 3};
        arr[5] = new int[]{3, 0, 3, 3, 3, 0, 0, 0, 0, 0, 3, 0, 3, 3, 0, 0, 0, 3};
        arr[6] = new int[]{3, 0, 0, 3, 3, 3, 3, 3, 3, 0, 0, 0, 3, 3, 3, 3, 0, 3};
        arr[7] = new int[]{3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
        arr[start.p.x][start.p.y] = 1;
        arr[end.p.x][end.p.y] = 2;
        MapInfo map = new MapInfo(arr, start, end);
        Stack<Node> result = AStarSort(map);
        ToolShow.printArr(map.map);
        ToolShow.log(result.toString());
    }

    /**
     * A-Star算法完整版每次从所有能走的点中选取最小的点A开始走，然后以A为节点动态更新所有能走的点的估价距离，并且保存A的上一个节点。当走到重点时，就可以通过保存的上一个节点，回退实际的最短路线。
     * A-Star算法完整版适用于有凹形障碍物的情形。
     * 操作步骤：
     * 1. 声明openList保存所有能走的点，声明closeList保存所有走过的点，把起点start添加到openList中
     * 2. 从openList中最小的点A开始走，把A添加到closeList中,然后重新查找所有可走的点。如果找到的点p之前不在openList中，把它直接添加到openList中；如果p之前就在openList中，判断经过节点A时p的估价L1与走原来方向时p的估价距离L2的大小，如果L1<L2，就改走A，把A设为p的parent节点，把L1设为p的估价距离，然后对openList重新排序；
     * 3. 重复步骤2，直到到达终点。
     * 4. 最后，根据终点end的parent，可以回推实际的最短路线。
     *
     * @param mapInfo 地图
     * @return 实际的最短路线
     */
    public static Stack<Node> AStarSort(MapInfo mapInfo) {
        if (mapInfo == null) {
            return null;
        }
        // 使用自动排序队列表示能走的点
        Queue<Node> openList = new PriorityQueue<>();
        // 已经走过的点
        List<Node> closeList = new ArrayList<>();
        openList.add(mapInfo.start);

        //如果有路可以走
        while (!openList.isEmpty()) {
            // 到达终点
            if (isColsed(closeList, mapInfo.end)) {
                //展示路线
                Stack<Node> result = drawPath(mapInfo);
                return result;
            }

            // 获取最小的节点
            Node now = openList.poll();
            //标记now已走过
            closeList.add(now);
            //寻找下一个可走的点
            addNext(mapInfo, openList, closeList, now);
        }
        return null;
    }

    /**
     * 寻找下一个可走的点
     *
     * @param mapInfo   地图
     * @param openList  能走的点
     * @param closeList 走过的点
     * @param now       当前点
     */
    private static void addNext(MapInfo mapInfo, Queue<Node> openList, List<Node> closeList, Node now) {
        //一维数组的下标
        int x = now.p.x;
        //二维数组的下标
        int y = now.p.y;
        addNext(mapInfo, openList, closeList, now, new Point(x - 1, y), DIRECT_VALUE);// 左
        addNext(mapInfo, openList, closeList, now, new Point(x, y - 1), DIRECT_VALUE);// 上
        addNext(mapInfo, openList, closeList, now, new Point(x + 1, y), DIRECT_VALUE);// 右
        addNext(mapInfo, openList, closeList, now, new Point(x, y + 1), DIRECT_VALUE);// 下
        addNext(mapInfo, openList, closeList, now, new Point(x - 1, y - 1), OBLIQUE_VALUE);// 左上
        addNext(mapInfo, openList, closeList, now, new Point(x + 1, y - 1), OBLIQUE_VALUE);// 右上
        addNext(mapInfo, openList, closeList, now, new Point(x - 1, y + 1), OBLIQUE_VALUE);// 左下
        addNext(mapInfo, openList, closeList, now, new Point(x + 1, y + 1), OBLIQUE_VALUE);// 右下
    }


    /**
     * 寻找下一个可走的点
     *
     * @param mapInfo   地图
     * @param openList  能走的点
     * @param closeList 走过的点
     * @param now       当前点
     * @param p         下一个准备走的点
     * @param value     从now到p的距离
     */
    private static void addNext(MapInfo mapInfo, Queue<Node> openList, List<Node> closeList, Node now, Point p, int value) {
        //先判断当前点能否添加到openList中:如果已经走过或者是障碍物，就不添加到openList中
        if (canAddOpen(mapInfo, closeList, p)) {
            Node end = mapInfo.end;
            // 从起点到p的距离
            int g = now.g + value;
            //从openList找p
            Node child = findOpen(openList, p);
            //如果是第一次遇到这个点
            if (child == null) {
                //p到终点的距离
                int h = calcH(end.p, p) * DIRECT_VALUE;
                //如果到达终点，就更新end的值，然后添加到openList中
                if (p.equals(end.p)) {
                    child = end;
                    //设置parent是为从end节点推路线图
                    child.parent = now;
                    child.g = g;
                    child.h = h;
                    //如果不是终点，就新建一个对象
                } else {
                    child = new Node(p, now, g, h);
                }
                //添加到openList中
                openList.add(child);
                //如果p已经在openList中，走当前路线的距离小于原来路线的距离，就用当前的g值替换原来的g值
            } else if (child.g > g) {
                child.g = g;
                //从now节点走，比原来的路线近，所以将它的parent设为now
                child.parent = now;
                // 重新排序，以便选择距离最小的点
                openList.add(child);
            }
        }
    }

    /**
     * 估值函数:使用曼哈顿方法计算距离，即用两条边长的和代表两点的距离，这样效率更高
     *
     * @param start 起点
     * @param end   终点
     * @return 两点的距离
     */
    private static int calcH(Point start, Point end) {
        return Math.abs(start.x - end.x) + Math.abs(start.y - end.y);
    }

    /**
     * 判断当前点能否添加到openList中
     *
     * @param mapInfo   地图
     * @param closeList 走过的点
     * @param p         当前点
     * @return 是否可走
     */
    private static boolean canAddOpen(MapInfo mapInfo, List<Node> closeList, Point p) {
        if (mapInfo == null || p == null) {
            return false;
        }

        if (p.x < 0 || p.x >= mapInfo.height || p.y < 0 || p.y >= mapInfo.width) {
            return false;
        }
        // 如果当前点是障碍物
        if (mapInfo.map[p.x][p.y] == BAR) {
            return false;
        }
        //如果已经走过
        if (isColsed(closeList, p.x, p.y)) {
            return false;
        }
        return true;
    }

    /**
     * 判断节点是否走过
     */
    public static boolean isColsed(List<Node> list, Node node) {
        if (list == null || list.isEmpty() || node == null || node.p == null) {
            return false;
        }
        return isColsed(list, node.p.x, node.p.y);
    }

    /**
     * 判断节点是否走过
     */
    public static boolean isColsed(List<Node> list, int x, int y) {
        if (list == null || list.isEmpty()) {
            return false;
        }
        for (Node node : list) {
            if (node.p.x == x && node.p.y == y) {
                return true;
            }
        }
        return false;
    }

    /**
     * 判断节点是否可以走
     */
    public static Node findOpen(Queue<Node> list, Point p) {
        if (list == null || list.isEmpty() || p == null) {
            return null;
        }
        for (Node node : list) {
            if (node.p.x == p.x && node.p.y == p.y) {
                return node;
            }
        }
        return null;
    }

    /**
     * 回推实际的最短路线
     *
     * @param mapInfo 地图
     * @return 实际的最短路线
     */
    public static Stack<Node> drawPath(MapInfo mapInfo) {
        if (mapInfo == null || mapInfo.map == null || mapInfo.end == null) {
            return null;
        }
        //保存路线
        Stack<Node> stack = new Stack<>();
        Node end = mapInfo.end;
        while (end != null) {
            stack.push(end);
            Point p = end.p;
            mapInfo.map[p.x][p.y] = PATH;
            end = end.parent;
        }
        return stack;
    }

    /**
     * 点
     */
    public static class Point {
        //一维数组的下标
        int x;
        //二维数组的下标
        int y;

        public Point(int x, int y) {
            super();
            this.x = x;
            this.y = y;
        }

        @Override
        public boolean equals(Object o) {
            if (o == null) {
                return false;
            }

            if (o instanceof Point) {
                Point p = (Point) o;
                if (p.x == x && p.y == y) {
                    return true;
                }
            }
            return false;
        }

        @Override
        public String toString() {
            return "Point [x=" + x + ", y=" + y + "]";
        }

    }

    /**
     * 节点
     */
    public static class Node implements Comparable<Node> {
        //当前点
        public Point p;
        //设置parent是为从end节点推路线图
        public Node parent;
        // 从起点到当前点的实际距离
        public int g;
        // 从当前点到终点的估价距离，用曼哈顿距离表示，只用于比较，提升效率
        public int h;

        public Node(int x, int y) {
            this.p = new Point(x, y);
        }

        public Node(Point p, Node parent, int g, int h) {
            super();
            this.p = p;
            this.parent = parent;
            this.g = g;
            this.h = h;
        }

        @Override
        public int compareTo(Node node) {
            if (node == null) {
                return -1;
            }

            if (node.g + node.h > g + h) {
                return -1;
            } else if (node.g + node.h < g + h) {
                return 1;
            }
            return 0;
        }

        @Override
        public String toString() {
            return "Node [p=" + p + ", g=" + g + ", h=" + h + "]";
        }
    }

    /**
     * 地图信息
     */
    public static class MapInfo {
        //地图
        int[][] map;
        //地图的宽
        int width;
        //地图的高
        int height;
        //起点
        Node start;
        //终点
        Node end;

        public MapInfo(int[][] map, Node start, Node end) {
            super();
            if (Tool.isEmpty(map)) {
                return;
            }
            this.map = map;
            this.width = map[0].length;
            this.height = map.length;
            this.start = start;
            this.end = end;
        }
    }
}
